// ==============================================
// =      U.A.V.P Brushless UFO Controller      =
// =           Professional Version             =
// = Copyright (c) 2007 Ing. Wolfgang Mahringer =
// ==============================================
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with this program; if not, write to the Free Software Foundation, Inc.,
//  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
// ==============================================
// =  please visit http://www.uavp.org          =
// =               http://www.mahringer.co.at   =
// ==============================================

// The PID controller algorithm

#pragma codepage=1
#include "c-ufo.h"
#include "bits.h"

// Math Library
#include "mymath16.h"


// compute the correction adders for the motors
// using the gyro values (PID controller)
// for the axes Roll and Pitch
void PID(void)
{
#ifdef NO_SENSORS
return;
#endif

	if(	BiasZeroCount > 0 )  // NO PID while averaging the gyro bias values
		return;
		
		
//	CheckLISL();	// get the linear sensors data, if available

// PID controller
// E0 = current gyro error
// E1 = previous gyro error
// Sum(Ex) = integrated gyro error, sinst start of ufo!
// A0 = current correction value
// fx = programmable controller factors
//
// for Roll and Pitch:
//       E0*fP + E1*fD     Sum(Ex)*fI
// A0 = --------------- + ------------
//            16               256


//  ###############  ROLL  #####################
//	Rl  = - Roll_Rate_8;
//	Rl += Roll_Rate_8_prev;


	// The Derivative term  uses the delta of  the rates of roll  between itterations  
	Rl = Roll_Rate_8_prev - Roll_Rate_8;
	Rl *= (long)RollDiffFactor;

	// The proportional term 
	Rl += (long)Roll_Rate_8 * (long)RollPropFactor;

	Rl += 8; // round up and divide by 16 
	Rl >>= 4;
 
	// Integral part for Roll
	Rp = Roll_Angle >> 2; // make 8 bit so that " I"  can be bigger 
	Rp = Rp * (long)RollIntFactor;
	Rp += 128;
	Rl += (int)Rp.high8;

	// Add stick signal
	Rl += IRoll;


// ###############  PITCH  #####################
//	Nl  = - Pitch_Rate_8;
//	Nl += Pitch_Rate_8_prev;

	// Derivative  
	Nl = Pitch_Rate_8_prev - Pitch_Rate_8;
	Nl *= (long)PitchDiffFactor;

	// Proportional 
	Nl += (long)Pitch_Rate_8   * (long)PitchPropFactor;
	
	Nl += 8;
	Nl >>= 4;	// divide rounded by 16

	// Integral 
	Np= Pitch_Angle >> 2; // make 8 bit so that " I"  can be bigger 
	Np = Np * (long) PitchIntFactor;
	Np += 128;
	Nl += (int)Np.high8;

	// Add  stick signal
	Nl += IPitch;
	
// PID controller for Yaw (Heading Lock)
//       E0*fp + E1*fD     Sum(Ex)*fI
// A0 = --------------- + ------------
//             16              256

// ##############  YAW  ######################
// Apply Integral factor 
// stick input IYaw is added to  Gyro YawSamples first so that YawAngle in the end stays at 0 during a yaw turn  
	Tl = Yaw_Angle >> 2; // make 8 bit so that " I"  can be bigger 

// Note: YawIntFactor must alwaqys be positive otherwise the math below fails 	
	niltemp = 32500 / (long) YawIntFactor;		// limiter so that we don't overflow in the following multiplication  
	if ( Tl > niltemp )
		Tl = niltemp;
    
	niltemp = -niltemp;		// and the negative side
    if ( Tl < niltemp )
		Tl = niltemp;
	
	Tl *= (long)YawIntFactor;
	Tl += 128;		// divide rounded by 256
	Tl = (int)Tl.high8;

//Apply  Derivative factor 
	Tp = Yaw_Rate_8_prev - Yaw_Rate_8;
	Tp *= (long)YawDiffFactor;
	
// Apply Proportional factor	
	Tp += (long)Yaw_Rate_8  * (long)YawPropFactor;
	Tp += 8;
	Tp >>= 4;	// divide rounded by 16
	Tl += Tp;

	NegFact = -YawLimit;
	if( Tl < NegFact ) Tl = NegFact;
	if( Tl > YawLimit ) Tl = YawLimit;
	
// Diagnostic outputs on the LEDS 	
	if( IntegralTest|| CompassTest )
	{
		ALL_LEDS_OFF;
		
		if( CompassTest )
		{
			if( CurDeviation > 0 )
				LedGreen_ON;
			if( CurDeviation < 0 )
				LedRed_ON;
				
			
			if( AbsDirection > COMPASS_MAX )
				LedYellow_ON;
		}

		if( IntegralTest )
		{
			if( (int)Roll_Angle.high8 > 0 )
			{
				LedRed_ON;
			}
			if( (int)Roll_Angle.high8 < -1 )
			{
				LedGreen_ON;
			}

			if( (int)Pitch_Angle.high8 >  0 )
			{
				LedYellow_ON;
			}
			if( (int)Pitch_Angle.high8 < -1 )
			{
				LedBlue_ON;
			}
		}
	}

}
